#include "SimpleCollisionSystem.h"
#include "OOBB3Collision.h"
#include "Game.h"
#include "Timer.h"
#include "RBTYPE.h"
#include "RBSphere.h"
#include "RigidBodySquare.h"
#include "RBSphereToRBCubeCollision.h"


void SimpleCollisionSystem::Update(GameObjectMap* objs)
{
	for (GameObjectMap::iterator it = objs->begin(); it != objs->end(); ++it)
	{
		//std::cout << it->first << std::endl;

		GameObjectMap::iterator it2 = it;
		++it2;

		for ( ; it2 != objs->end(); ++it2)
		{
			//std::cout << it2->first << std::endl;

			GameObject* go1 = it->second;
			GameObject* go2 = it2->second;

			if (CheckCollisionOptimised(go1->GetOOBB3(), go2->GetOOBB3()))
			{
				//TheGame::Instance()->HandleCollisions(go1, go2);
			}
			
			//if (go1->GetRBType() == CUBE && go2->GetRBType() == CUBE)
			//{
			//	RigidBodySquare* rb1 = dynamic_cast<RigidBodySquare*>(go1->GetRB());
			//	RigidBodySquare* rb2 = dynamic_cast<RigidBodySquare*>(go2->GetRB());

			//	if (CheckCollisionOptimised(go1->GetOOBB3(), go2->GetOOBB3()))
			//	{
			//		rb2->AddForce(-rb2->GetMass() * rb2->GetAccleration() * 1.02f);
			//		//rb1->AddForce(rb2->GetForce());

			//		TheGame::Instance()->HandleCollisions(go1, go2);
			//	}
			//}
			//else if (go1->GetRBType() == CUBE && go2->GetRBType() == SPHERE)
			//{
			//	//RigidBody2* rb2 = go2->GetRB();
			//	//RBSphere* rb = dynamic_cast<RBSphere*>(rb2);

			//	//// later change this so that dyanmic_cast to rbsquare instead of using game obecjts oobb3
			//	//rb2 = go1->GetRB();
			//	//RigidBodySquare* rbSquare = dynamic_cast<RigidBodySquare*>(rb2);
			//	//
			//	//if (CheckCollision(go1->GetOOBB3(), rb))
			//	//{
			//	//	// force = mass * acceleration
			//	//	// force = mass * (newVel - oldVel)
			//	//	//XMVECTOR newForce = rb2->GetMass() * (rb2->GetVelocity() - rb2->GetOldVelocity());
			//	//	XMVECTOR newForce = rb->GetMass() * (rb->GetVelocity() - rb->GetOldVelocity());
			//	//	rb->AddForce(-rb->GetForce() * 1.02f);

			//	//	TheGame::Instance()->HandleCollisions(go1, go2);
			//	//}
			//}
			//else if (go1->GetRBType() == SPHERE && go2->GetRBType() == CUBE)
			//{

			//}
		}
	}
}